#include "./drop_cup_motor.h"
#include "../../utils/progress.h"
#include "../analog.h"
#include "./sensor.h"
#include "../../device/gpio.h"

namespace driver {

using namespace device::gpio;
using namespace utils;

using DIR1 = PC<8>;
using DIR2 = PC<9>;
using DROP1 = PC<10>;
using DROP2 = PC<11>;

DropCupMotor::DropCupMotor() {
    RCC->APB2ENR |= RCC_APB2ENR_IOPCEN;

    config_pp_out<DIR1, DIR2, DROP1, DROP2>();
    set_dir(Dir::BRAKE);
}

Error DropCupMotor::drop_cup1() {
    Error err;
    Progress prog;
    uint8_t lock_count = 0;
    bool flag = false;

    set_dir(Dir::CCW);
    DROP1::set();

    for (;;) {
        bool val = sensor->is_cup1_sensor();
        if ((!flag) && (!val)) {
            flag = true;
        }
        if (flag && val) {
            break;
        }
        
        if (prog.is_after(500) && analog->is_drop_motor_locked()) {
            lock_count ++;
            if (lock_count >= 5) {
                err = ec::PIPE_LINE_DROP_CUP1_MOTOR_LOCKED;
                break;
            }
        } else {
            lock_count = 0;
        }

        if (prog.is_after(DROP_TIMEOUT)) {
            err = ec::PIPE_LINE_DROP_CUP1_MOTOR_TIMEOUT;
            break;
        }
        
        osDelay(50);
    }

    DROP1::clr();
    set_dir(Dir::BRAKE);

    return err;
}

Error DropCupMotor::drop_cup2() {

    Error err;
    Progress prog;
    uint8_t lock_count = 0;
    bool flag = false;

    set_dir(Dir::CCW);
    DROP2::set();

    for (;;) {
        bool val = sensor->is_cup2_sensor();
        if ((!flag) && (!val)) {
            flag = true;
        }
        if (flag && val) {
            break;
        }
        
        if (prog.is_after(500) && analog->is_drop_motor_locked()) {
            lock_count ++;
            if (lock_count >= 5) {
                err = ec::PIPE_LINE_DROP_CUP2_MOTOR_LOCKED;
                break;
            }
        } else {
            lock_count = 0;
        }

        if (prog.is_after(DROP_TIMEOUT)) {
            err = ec::PIPE_LINE_DROP_CUP2_MOTOR_TIMEOUT;
            break;
        }
        
        osDelay(50);
    }

    DROP2::clr();
    set_dir(Dir::BRAKE);

    return err;
}

Error DropCupMotor::drop_cup1_with_check() {
    Error err;
    if (sensor->is_drop_cup1()) {
        err = ec::PIPE_LINE_DROP_CUP1_POSITION_ERR;
        goto finish;
    }
    err = drop_cup1();
    if (err) {
        goto finish;
    }
    if (!sensor->is_drop_cup1()) {
        err = ec::PIPE_LINE_DROP_CUP1_FAIL;
        goto finish;
    }
finish:
    return err;
}

Error DropCupMotor::drop_cup2_with_check() {
    Error err;
    if (sensor->is_drop_cup2()) {
        err = ec::PIPE_LINE_DROP_CUP2_POSITION_ERR;
        goto finish;
    }
    err = drop_cup2();
    if (err) {
        goto finish;
    }
    if (!sensor->is_drop_cup2()) {
        err = ec::PIPE_LINE_DROP_CUP2_FAIL;
        goto finish;
    }
finish:
    return err;
}


void DropCupMotor::set_dir(Dir dir) {
    switch (dir) {
        case Dir::CW:
            DIR1::clr();
            DIR2::set();
        break;

        case Dir::CCW:
            DIR1::set();
            DIR2::clr();
        break;

        default:
            DIR1::clr();
            DIR2::clr();
        break;
    }
}

}
